#include "MK20D5.h"           // Device header
extern void benchmark(void);  // Benchmark function
volatile unsigned count;			// Performance counter
volatile unsigned cntmsb;     // FTM0 MSB counter

#define BENCH_RUNS 100        /* Not really needed unless OS used,
															   but makes for a nice blink rate ;) */
// FTM0 overflow (MSB) counter
// Note: FTM0 is only 16 bits, this is used to extend to 32 bits.
//       The timer must be is stopped before reading.
void FTM0_IRQHandler(void) {
	FTM0->SC &= ~FTM_SC_TOF_MASK; // Clear overflow
	cntmsb++; // MSB counter
}

// System setup
void setup(void) {
  // Clock gates 
	SIM->SCGC5 |= SIM_SCGC5_PORTA_MASK | SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTD_MASK;
	SIM->SCGC6 |= SIM_SCGC6_FTM0_MASK | SIM_SCGC6_FTM1_MASK;
  // Configure GPIO
	PORTD->PCR[4] = (1UL << 8);
  PORTC->PCR[3] = (1UL << 8);
  PORTA->PCR[2] = (1UL << 8);
	// Outputs off
  PTD->PDOR = (1UL << 4); PTC->PDOR = (1UL << 3); PTA->PDOR = (1UL << 2);
	// Direction
  PTD->PDDR = (1UL << 4); PTC->PDDR = (1UL << 3); PTA->PDDR = (1UL << 2);
	// Set up FTM for profiling at 48MHz
	FTM0->MOD = 0xFFFF;
  NVIC_EnableIRQ(FTM0_IRQn);
}

// Reset benchmark timer
__inline
void time_reset(void) {
	FTM0->SC = 0; // Disable
	cntmsb = 0; // Clear MSB
	FTM0->CNT = 0; // Clear LSB
  FTM0->SC = FTM_SC_CLKS(1) | FTM_SC_TOIE_MASK; // Enable
}

// Read benchmark timer
__inline
unsigned time_read(void) {
  FTM0->SC = 0; // Disable
	return FTM0->CNT | (cntmsb << 16); // Combine MSB|LSB
}

// LED indication - red, green, blue
void led_r(void) { PTA->PSOR = (1UL << 2); PTD->PSOR = (1UL << 4); PTC->PCOR = (1UL << 3); }
void led_g(void) { PTC->PSOR = (1UL << 3); PTA->PSOR = (1UL << 2); PTD->PCOR = (1UL << 4); }
void led_b(void) { PTD->PSOR = (1UL << 4); PTC->PSOR = (1UL << 3); PTA->PCOR = (1UL << 2); }

// Entry
int main (void) {
  setup(); // Board setup
	
	unsigned toggle = 0;
	while(1) {
		
		// Toggle RED/GREEN to indicate activity
		toggle = !toggle;
		if(toggle) led_r(); else led_g();

		// Run benchmark
		unsigned ack = 0;
		for(unsigned n = 0; n < BENCH_RUNS; n++) {
			time_reset(); // Reset timer
			benchmark(); // Run test
			ack += time_read(); // Accumulate time
		}
		// Divide to get time of a single run
		count = ack / BENCH_RUNS;
		// Set BLUE to indicate done
		led_b();

		// BREAKPOINT HERE - check "count"
		count = count;
		// For -O3, optimize for time, cross module optimization:
		//   Original count: 0x279AD
		//   Translates to : 3.4mS   = 0x279AD/48MHz

	}
}
